/**
 * @file serial_sim.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief 串口数据仿真节点
 * @version 0.1
 * @date 2020-07-21
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "global.h"

#include "serial_driver.h"

#include<iostream>
#include<fstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "SERIAL_SIM");

  ros::NodeHandle serial;

  ros::Publisher send = serial.advertise<storm_car::car_status>("serial_receive_data", 1000);
  
  ros::Rate loop_rate(10);

  storm_car::car_status msg;

  std::vector<std::string> v_temp;
  std::string temp;
  std::ifstream fin(FILEPATH);

  if(!fin)
  {
    ROS_INFO("OPEN FILE FAILED");
    return false;
  }

  while (ros::ok())
  {
    if (getline(fin, temp))
    {
      SplitString(temp, v_temp, ",");
      msg.heading = StringToDouble(v_temp[0].c_str());
      msg.lattitude = StringToDouble(v_temp[1].c_str());
      msg.longitude = StringToDouble(v_temp[2].c_str());
      msg.velocity = StringToDouble(v_temp[3].c_str());

      send.publish(msg);

      temp.clear();
      v_temp.clear();
      msg.heading = 0;
      msg.lattitude = 0;
      msg.longitude = 0;
      msg.velocity = 0;
    }
    else
    {
      break;
    }
    loop_rate.sleep();
    ros::spinOnce();
  }

  fin.close();
  ROS_INFO("plan data is all loaded!!");
  
  ros::spin();

  return 0;
}